#pragma once
#include "motor/Motor.h"
#include "PIDProvider.h"

#define LOG_TAG "MotorController"

#include <elog.h>

#define MAX_MOTOR_DUTY 999

/**
 * @brief 电机
 */
struct Motor L1;
struct Motor L2;
struct Motor R1;
struct Motor R2;

/**
 * @brief 码盘线数(挖孔数)
 */
#define CODE_WHEEL_LINE 20

/**
 * @brief 码盘测速更新间隔(ms)
 */
#define CODE_WHEEL_UPDATE_INTERVAL 0.2

/**
 * @brief 码盘计数
 */
uint32_t Wheel_L1;
uint32_t Wheel_L2;
uint32_t Wheel_R1;
uint32_t Wheel_R2;

/**
 * @brief 电机转速
 */
float Speed_L1;
float Speed_L2;
float Speed_R1;
float Speed_R2;

/**
 * @brief 电机转速(目标)
 */
float Speed_L;
float Speed_R;

/**
 * @brief 电机PID参数
 */
struct PIDParam Motor_PIDParam = {
    .P = 130,
    .D = 64
};

/**
 * @brief 根据目标速度返回电机转动方向
 * 
 * @param speed 目标速度
 * @return MotorMode 转动方向
 */
MotorMode get_motor_mode_from_speed(float speed)
{
    if (speed == 0)
        return Brake;
    else if (speed > 0)
        return Forward;
    else
        return Backward;
}

/**
 * @brief 根据码盘转速调整电机Duty
 */
void update_code_wheel_speed()
{
    Speed_L1 = (float)Wheel_L1 / CODE_WHEEL_LINE / CODE_WHEEL_UPDATE_INTERVAL;
    Speed_L2 = (float)Wheel_L2 / CODE_WHEEL_LINE / CODE_WHEEL_UPDATE_INTERVAL;
    Speed_R1 = (float)Wheel_R1 / CODE_WHEEL_LINE / CODE_WHEEL_UPDATE_INTERVAL;
    Speed_R2 = (float)Wheel_R2 / CODE_WHEEL_LINE / CODE_WHEEL_UPDATE_INTERVAL;

    Wheel_L1 = Wheel_L2 = Wheel_R1 = Wheel_R2 = 0;

    set_motor_mode(&L1, get_motor_mode_from_speed(Speed_L));
    set_motor_mode(&L2, get_motor_mode_from_speed(Speed_L));
    set_motor_mode(&R1, get_motor_mode_from_speed(Speed_R));
    set_motor_mode(&R2, get_motor_mode_from_speed(Speed_R));

    log_d("Speed_L Mode:%d", get_motor_mode_from_speed(Speed_L));
    log_d("Speed_R Mode:%d", get_motor_mode_from_speed(Speed_R));

    update_motor_speed(&L1, abs(Speed_L) - Speed_L1);
    update_motor_speed(&L2, abs(Speed_L) - Speed_L2);
    update_motor_speed(&R1, abs(Speed_R) - Speed_R1);
    update_motor_speed(&R2, abs(Speed_R) - Speed_R2);
}

#pragma region WaveGenerator

float MaxSpeed = 0;
float WaveFreq = 0.08;
float BaseSpeed = 3;

void SpeedAsSinWave()
{
    Speed_L = Speed_R = MaxSpeed * sin(2 * PI * WaveFreq * HAL_GetTick() / 1000) + BaseSpeed;
}

#pragma endregion
